/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ticks.PID.subsystems;

import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SendablePIDController;
import ticks.PID.commands.ArmWithPotentiometer;

/**
 *
 * @author Jared
 */
public class BetterPID extends Subsystem implements PIDSource{ //Implements PIDSource..Means that it has a method that
    //Returns a pid sensor value.  This is part of writing the code a longer way.
    //The PID controller is smart, and can recognize common sensors/motors as inputs/outputs for the pid loop
    //so writing the code is easy.
    //Althogh the encoder can be used the same way as the motor is, it's written 
    //A longer way to show how you could use a sensor that's not normally compatible with pid.
    SpeedController armMotor; //These statements "declare" the names.  They don't actuall make an encoder.
    Encoder armEncoder; //encoder tells you the angle of the arm
    SendablePIDController pid;  //This is also a declaration.  A sendable PID controller
    //is similar to a normal one, but the gains can be tuned remotely on the dashboard
    //so we set the default gains to be zero.
    
    public BetterPID(){//This is a constructor.  It has initialization tasks in it and it sets up the stuff 
        //that was declared earlier
        armMotor = new Victor(1); //Victor is connected to port 1 on Digital Sidecar
        armEncoder = new Encoder(1, 2);
        armEncoder = new Encoder(1, 2, false, CounterBase.EncodingType.k4X); //encoder(tells angle) on ports 1/2. 
        //the other parts have to do with triggering and signal analysis 
        //4x means that it triggers on rising/falling edge of both channels, but that's not really important
        pid = new SendablePIDController(0, 0, 0, this, armMotor);  
        //Creates a new pid controller with default gains of zero, zero, and zero
        //The gains can be changed on the dashboard software running on the computer
        //the word "this" is a java keyword which represents this class we are in
        //"this" tells the pid loop to look to this class for a sensor that gives the position of the arm
        //because this class implemetns pidsource, the loop knows that this class must hav a method called
        //pidGet which will return the value of the encoder
        //the word "armMotor" tells the pidloop that it will use that motor as the output
        //The speedcontroller class is set up so that it can be easily used with the pid controller.
        armEncoder.setDistancePerPulse(0.01);
        //Setting the distance per pulse to 1/pulses per revolution (found on encoder packaging)
        //scales the encdoer so that one turn has a distance of 1
        //It is also possible to calculate a value so that the encoder will return the angle of the arm
        //by multiplying that number by 1.
        armEncoder.start(); //starts the encoder counting stuff on FPGA
        pid.setInputRange(-1, 1); //we'd change these based on the number of turns the arm can go
        pid.setOutputRange(-1, 1); //motor control is done between -1,1 so we always use these when using
        //positional pid
        pid.enable(); //starts the pid loop
        
    }
    
    public void setArmSetpoint(double setpoint){//this method is used to adjust the pid
        //setpoint for the pid loop.
        //This would get used by another part of the code
        pid.setSetpoint(setpoint);
    }
    
    public boolean armAtSetpoint(){
        return Math.abs(pid.getSetpoint()-this.pidGet())<0.1;  //Tells us if we are within 0.1 rotations (36 degrees)
        //of the setpoint.
        //Used to terminate the manual setpoint command.
    }
    
    

    public void initDefaultCommand() {
        setDefaultCommand(new ArmWithPotentiometer());
        //If no other command is running, this command will run.
        //ArmWithPotentiometer will make it so that the arm 
        //is controlled by a potentiometer connected
        //to our button board.
        //We could also make other commands which set the setpoint to a
        //specific value for certain tasks.
        //We could make one MoveArmTo(double position).
        //We could then use it in autonomous programming
        //If that command starts, it will interrupt this command
        //when that command finishes, it will return to this command
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }

    public double pidGet() {//This method returns the current value of the arm encoder.
        //the pid loop will automatically call this metthod and get that value
        //whenever it needs to
        
        return armEncoder.getDistance(); //returns the arm encoder distance!
    }
}
